#include "Comum.h"

extern Error_States errorCode;

/*******************************************************************************
 * @NAME:               PWM_Set_Motor                               	           *
 * @BRIEF:              Set the PWM for motors and direction					   *
 * @CREATED_BY:         Ricardo Sa                                              *
 * @CREATION_DATE:      29 mar 2014                                             *
 * @SPEC_REF:                                                                   *
 * @LIB_REF:                                                                    *
 * @LIB_REUSE_%:                                                                *
 *******************************************************************************/
void Drv_PWM_Set_Motor(INT8U direction, INT16U pulse)
{

	/* Configure the pwm for motors */
	HAL_config_timer(0, 50);

	if(pulse > 100)
	{
		errorCode = Err_6;
	}
	pulse = pulse * 200;
	/* Sets the pwm for motors */
	HAL_set_pwm (0, pulse);
	HAL_set_pwm (1, pulse);

	switch(direction)
	{

	case RIGHT:
		UARTprintf("RIGHT\n");

		HAL_Write_Pins('E', 4, HIGH);
		HAL_Write_Pins('E', 5, LOW);

		break;

	case LEFT:
		UARTprintf("LEFT\n");

		HAL_Write_Pins('E', 4, LOW);
		HAL_Write_Pins('E', 5, HIGH);

		break;

	case FRONT:
		UARTprintf("FRONT\n");

		HAL_Write_Pins('E', 4, HIGH);
		HAL_Write_Pins('E', 5, HIGH);

		break;
	case 0:
		HAL_Write_Pins('E', 4, LOW);
		HAL_Write_Pins('E', 5, LOW);

		break;

	default:

		errorCode = Err_6;
		break;

	}

	DelayTask(100);


}

/*******************************************************************************
 * @NAME:               PWM_Servo_Set                               	        *
 * @BRIEF:              Set the PWM for servo motor							    *
 * @CREATED_BY:         Ricardo Sa                                              *
 * @CREATION_DATE:      29 mar 2014                                             *
 * @SPEC_REF:                                                                   *
 * @LIB_REF:                                                                    *
 * @LIB_REUSE_%:                                                                *
 *******************************************************************************/
void Drv_PWM_Set_Servo(INT16U angle_servo)
{
	INT16U pulse;

	if(angle_servo <=180)
	{

		/*  Set period for PB0   */
		HAL_config_timer(1, 50);

			/*  Converts degrees in duty-cycle for servo  */
			pulse = (((angle_servo*180)/18)+530);
			/*  Set duty for PB0  */
			HAL_set_pwm (2, pulse);


	}
	else
	{
		errorCode = Err_7;
	}

}

/*******************************************************************************
 * @NAME:               ERR_SetRedLED                               	        *
 * @BRIEF:              Blinks the red LED									    *
 * @CREATED_BY:         Ricardo Sa                                              *
 * @CREATION_DATE:      29 mar 2014                                             *
 * @SPEC_REF:                                                                   *
 * @LIB_REF:                                                                    *
 * @LIB_REUSE_%:                                                                *
 *******************************************************************************/
void Drv_ERR_Set_RedLED()
{

	HAL_config_ports('F', 1, HAL_OUTPUT);

	HAL_Write_Pins('F', 1, HIGH);
	DelayTask(100);

	HAL_Write_Pins('F', 1, LOW);
	DelayTask(100);

}


/*******************************************************************************
 * @NAME:               Alg_Sinalizador                                         *
 * @BRIEF:              Set the sinalizador      							   *
 * @CREATED_BY:         Ricardo Sa                                              *
 * @CREATION_DATE:      29 mar 2014                                             *
 * @SPEC_REF:                                                                   *
 * @LIB_REF:                                                                    *
 * @LIB_REUSE_%:                                                                *
 *******************************************************************************/
void Drv_Set_Sinalizador(INT16U value)
{
	if(value > 1)
	{
		errorCode = Err_8;
	}
	HAL_Write_Pins('B', 5, value);


}
